#include "map_local.h"
#include <iostream>
#include <algorithm>

MapLocal::MapLocal(int width, int height, float grid_size)
    : width_(width), height_(height), size_(width * height), grid_size_(grid_size)
{
    map_cur_ = new unsigned char[size_];
    std::fill(map_cur_, map_cur_ + size_, 128);

    map_pre_ = new unsigned char[size_];
    std::fill(map_pre_, map_pre_ + size_, 128);
}

MapLocal::~MapLocal() {
    delete[] map_pre_;
    delete[] map_cur_;
}


void MapLocal::getPriorMap(const std::vector<unsigned char>& map_ptr, int width, int height, const cv::Point3f& fusepose_xyz, cv::Point3f& fusepose_rpy)
{
    // 获取全局栅格坐标
    cv::Point fuse_grid = world2grids(fusepose_xyz);

    

}


void MapLocal::updateFromGlobalMap(const MapGlobal& global_map, int robot_x, int robot_y) 
{
    int half_width = width_ / 2;
    int half_height = height_ / 2;

    // for (int y = 0; y < height_; ++y) {
    //     for (int x = 0; x < width_; ++x) {
    //         int global_x = robot_x - half_width + x;
    //         int global_y = robot_y - half_height + y;

    //         if (global_x >= 0 && global_x < global_map.getWidth() &&
    //             global_y >= 0 && global_y < global_map.getHeight()) {
    //             map_[y * width_ + x] = global_map.getCell(global_x, global_y);
    //         } else {
    //             map_[y * width_ + x] = 255;
    //         }
    //     }
    // }


}


std::vector<unsigned char> MapLocal::getUpdatedLocalMapData() const 
{
    return std::vector<unsigned char>(map_cur_, map_cur_ + size_);
}


void  MapLocal::transformCoords(const cv::Point3f& local_points, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy) 
{
        float x_local = local_points.x;
        float y_local = local_points.y;
        float x = fusepose_xyz.x;
        float y = fusepose_xyz.y;
        float yaw = fusepose_rpy.z;

        // 使用 rotatePoint 函数进行点的旋转
        cv::Point2f local_point(x_local, y_local);
        cv::Point2f center_point(x, y);
        cv::Point2f rotated_point = rotatePoint(local_point, center_point, yaw);

        float x_world = rotated_point.x;
        float y_world = rotated_point.y;

        int global_obts_grid_x = static_cast<int>(x_world / GRID_SIZE);
        int global_obts_grid_y = static_cast<int>(y_world / GRID_SIZE);


        int centerx = static_cast<int>(x / GRID_SIZE);
        int centery = static_cast<int>(y / GRID_SIZE);

        int local_grid_x = global_obts_grid_x - centerx + LOCAL_CENTER;
        int local_grid_y = global_obts_grid_y - centery + LOCAL_CENTER;

        // return std::make_tuple(cv::Point2f(x_world, y_world), 
        //                        cv::Point(grid_x, grid_y), 
        //                        cv::Point(global_obts_grid_x, global_obts_grid_y));


        frame_data_cur_.ai_obts.push_back(local_points);
        frame_data_cur_.world_pose_obts.push_back(cv::Point3f(x_world, y_world, 0.0f));
        frame_data_cur_.world_grid_obts.push_back(cv::Point(global_obts_grid_x, global_obts_grid_y));
        frame_data_cur_.local_grid_obts.push_back(cv::Point(local_grid_x, local_grid_y));


}


void MapLocal::transformPoints(std::vector<cv::Point3f> pts, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy)
{

    // 当前帧
    frame_data_cur_.fusepose_xyz = fusepose_xyz;
    frame_data_cur_.fusepose_rpy = fusepose_rpy;


    // 将当前帧的障碍物转换到世界坐标，栅格坐标和局部坐标
    for (size_t i = 0; i < pts.size(); ++i) {
        auto p = pts[i];
        // p.x = p.x + 0.513;
        // p.x = p.x + 0.513;
        cv::Point3f pp(p.x, p.y, p.z);
        transformCoords(pp, fusepose_xyz, fusepose_rpy);
    }

}

void MapLocal::update(const std::vector<unsigned char>& map_ptr, std::vector<cv::Point3f> pts, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy, int idx)
{

    // 获取先验地图
    // map_ref_ = map_ptr;
    std::cout << " global : " <<  (int)map_ptr[0]<< std::endl;

    // 转换上一帧的数据，同时清空 map_cur_;4
    handlePreFrameData(fusepose_xyz, fusepose_rpy, idx);

    // 处理当前帧的数据
    handleCurFrameData(pts, fusepose_xyz, fusepose_rpy, idx);

    std::cout << "cur._len_: " << frame_data_cur_.unique_index.size() << std::endl;
    std::cout << "pre._len_: " << frame_data_pre_.unique_index.size() << std::endl;
    std::cout << "tmp._len_: " << frame_data_tmp_.unique_index.size() << std::endl;

    auto pre_un_obts = select_points_by_indices(frame_data_tmp_.world_grid_obts, frame_data_tmp_.unique_index);
    auto cur_un_obts = select_points_by_indices(frame_data_cur_.world_grid_obts, frame_data_cur_.unique_index);

    auto result =  find_common_point_indices(pre_un_obts, cur_un_obts);
    std::vector<int> pre_obts_indexs = result.first;
    std::vector<int> cur_obts_indexs = result.second;   
    // auto [pre_obts_indexs, cur_obts_indexs] = find_common_point_indices(pre_un_obts, cur_un_obts);

    // new
    for(int i = 0; i < frame_data_cur_.unique_index.size(); i++)
    {
        int value = frame_data_cur_.unique_index[i];
        if (isValueInVector(cur_obts_indexs, value) == true)
        {
            // cv::Point pre_point = frame_data_pre_.local_grid_obts[value];
            cv::Point cur_point = frame_data_cur_.local_grid_obts[value];
            map_cur_[pointToIndex(cur_point)] = updateProbability(map_cur_[pointToIndex(cur_point)], 0.8f, 0.2f);
        }
        else
        {
            cv::Point cur_point = frame_data_cur_.local_grid_obts[value];
            map_cur_[pointToIndex(cur_point)] = updateProbability(map_cur_[pointToIndex(cur_point)], 0.7f, 0.3f);
        }

    }

    // remaining
    for (size_t i = 0; i < frame_data_tmp_.unique_index.size(); i++)
    {
        int value = frame_data_tmp_.unique_index[i];
        if (isValueInVector(pre_obts_indexs, value) == false)
        {
            auto cam_local = frame_data_cur_.car_pose[0];
            auto obt_local = frame_data_tmp_.local_grid_obts[value];
            int  hasNum = frame_data_tmp_.unique_index.size();
            if(calculateAngle(cam_local, obt_local) == true)
            {
                // clear in visual
                cv::Point cur_point = frame_data_tmp_.local_grid_obts[value];
                unsigned char old = map_cur_[pointToIndex(cur_point)];
                unsigned char new_v = updateProbability(old, 0.45, 0.55);
                printf("cur_point [%d %d] , old %d, new %d\n",int(cur_point.x), int(cur_point.y), int(old), int(new_v));
                map_cur_[pointToIndex(cur_point)]  = new_v;
                // map_cur_[pointToIndex(cur_point)] = updateProbability(map_cur_[pointToIndex(cur_point)], 0.4f, 0.6f);
                if(map_cur_[pointToIndex(cur_point)] > 128)
                {
                    frame_data_cur_.world_pose_obts.push_back(frame_data_tmp_.world_pose_obts[value]);
                    frame_data_cur_.local_grid_obts.push_back(frame_data_tmp_.local_grid_obts[value]);
                    frame_data_cur_.world_grid_obts.push_back(frame_data_tmp_.world_grid_obts[value]);
                    frame_data_cur_.ai_obts.push_back(frame_data_tmp_.ai_obts[value]);
                    frame_data_cur_.unique_index.push_back(hasNum);
                    hasNum ++;

                }
            }
            else
            {
                    frame_data_cur_.world_pose_obts.push_back(frame_data_tmp_.world_pose_obts[value]);
                    frame_data_cur_.local_grid_obts.push_back(frame_data_tmp_.local_grid_obts[value]);
                    frame_data_cur_.world_grid_obts.push_back(frame_data_tmp_.world_grid_obts[value]);
                    frame_data_cur_.ai_obts.push_back(frame_data_tmp_.ai_obts[value]);
                    frame_data_cur_.unique_index.push_back(hasNum);
                    hasNum ++;
            }

            frame_data_cur_.ai_obts_len = hasNum; // ?

        }
        else
        {
            ;
        }
    }

    // 统计大于 128 的元素数量
    int count_greater_than_128 = 0;
    for (int i = 0; i < size_; ++i) {
        if (map_cur_[i] < 128) {
            map_cur_[i] = 128; 
        } else if (map_cur_[i] > 128) {
            ++count_greater_than_128; 
            std::cout << "i: (" << int(i/100)  << ", " << int(i%100) << ") " << int(map_cur_[i]) / 255.0 << std::endl;
        }
    }
    std::cout << std::endl;
    std::cout << "Number of elements greater than 128: " << count_greater_than_128 << std::endl;

    cv::Mat image(100, 100, CV_8UC3, cv::Scalar(255, 255, 255)); // 白色图像

    // 遍历数组，将值大于 128 的点绘制到图像中
    for (int x = 0; x < image.rows; x++) {         // 逻辑 x 朝下，对应图像的行
        for (int y = 0; y < image.cols; y++) {     // 逻辑 y 朝右，对应图像的列
            int index = x * image.cols + y;        // 索引 = 行 * 列数 + 列
            int value = map_cur_[index];
            if (value >= 128 && value <= 255) {
                // 灰度值计算（与方向无关）
                int grayValue = 255 - ((value - 128) * 255 / 127);
                // 访问图像时，x 是行，y 是列
                image.at<cv::Vec3b>(y, x) = cv::Vec3b(grayValue, grayValue, grayValue);
            }
        }
    }

    // draw
    drawCarPose(image, frame_data_cur_.car_pose);
    std::string name = "index_" + std::to_string(idx+1) + ".jpg";
    cv::Mat img_transposed;
    cv::transpose(image, img_transposed); // 进行转置
    cv::Mat img_resized;
    cv::resize(img_transposed, img_resized, cv::Size(img_transposed.cols * 1, img_transposed.rows * 1));
    cv::putText(img_resized, std::to_string(idx+1), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1.0, cv::Scalar(0, 255, 0), 1), cv::imwrite(name, img_resized);
    cv::imwrite(name, img_resized);


}



void MapLocal::handleCurFrameData(std::vector<cv::Point3f> pts, const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy, int sensor_index)
{
    frame_data_cur_.fusepose_xyz = fusepose_xyz;
    frame_data_cur_.fusepose_rpy = fusepose_rpy;

    // 处理当前障碍物点,现在所有点的值都被记录在了 frame_data_cur__;
    transformPoints(pts, fusepose_xyz, fusepose_rpy);
    calcCamLocalPose(fusepose_rpy);

    //获取唯一的栅格中的点 
    std::vector<int> unique_idenxs = get_unique_indexes(frame_data_cur_.world_grid_obts);
    frame_data_cur_.unique_index = unique_idenxs;
    frame_data_cur_.ai_obts_len  = unique_idenxs.size();

}




void MapLocal::handlePreFrameData(const cv::Point3f& fusepose_xyz, const cv::Point3f& fusepose_rpy, int sensor_index)
{

    frame_data_pre_.clear();
    frame_data_pre_ = frame_data_cur_;
    frame_data_tmp_.clear();
    frame_data_cur_.clear();

    std::memcpy(map_pre_, map_cur_, size_ * sizeof(unsigned char));
    std::fill(map_cur_, map_cur_ + size_, 128);


    if(sensor_index == 0)
    {
        // continue;
    }
    else
    {
        cv::Point cur_fuse_grid = world2grids(fusepose_xyz);
        int j = 0;
        for (size_t i = 0; i < frame_data_pre_.unique_index.size(); i++)
        {
            /* code */
            int un_idx = frame_data_pre_.unique_index[i];
            auto world_grid_obt = frame_data_pre_.world_grid_obts[un_idx];
            auto local_grid_obt = frame_data_pre_.local_grid_obts[un_idx];
            auto world_pose_obt = frame_data_pre_.world_pose_obts[un_idx];
            auto obts = frame_data_pre_.ai_obts[un_idx];
            // auto                 = frame_data_cur_.car_pose
            if(is_in_2m_range(world_grid_obt, cur_fuse_grid) == false)
            {
                ;
            }
            else
            {
                cv::Point new_point = world_grid_obt - cur_fuse_grid + cv::Point(LOCAL_CENTER, LOCAL_CENTER);
                printf("new_point: [%d, %d]\n", int(new_point.x), int(new_point.y));
                frame_data_tmp_.local_grid_obts.push_back(new_point);
                // frame_data_pre_.old_local_grid_obts.push_back(local_grid_obt);
                frame_data_tmp_.world_pose_obts.push_back(world_pose_obt);
                frame_data_tmp_.world_grid_obts.push_back(world_grid_obt);
                frame_data_tmp_.unique_index.push_back(j);
                j++;
                map_cur_[pointToIndex(new_point)] = map_pre_[pointToIndex(local_grid_obt)];
                frame_data_tmp_.ai_obts.push_back(obts);

            }
            frame_data_tmp_.car_pose = frame_data_pre_.car_pose;
            frame_data_tmp_.fusepose_rpy = frame_data_pre_.fusepose_rpy;
            frame_data_tmp_.fusepose_xyz = frame_data_pre_.fusepose_xyz;
            frame_data_tmp_.ai_obts_len = j;
        }
    }
}


void MapLocal::printMap() const 
{
    // for (int y = 0; y < height_; ++y) {
    //     for (int x = 0; x < width_; ++x) {
    //         std::cout << static_cast<int>(map_[y * width_ + x]) << " ";
    //     }
    //     std::cout << std::endl;
    // }
}


void MapLocal::calcCamLocalPose(const cv::Point3f& fusepose_rpy)
{
    float yaw = fusepose_rpy.z;
    int car_len = 666;
    int car_wid = 472;
    // int car_hei = 294;
    int car_cen = 513;
    float grid_size = 1 / 0.05f;

    // 假设 map_shape 是一个 std::pair<int, int> 或类似的结构
    int w = 1000; // 示例宽度
    int h = 1000; // 示例高度

    int x_top = static_cast<int>((666 - 513) / 1000.0f * grid_size) * (-1);
    int x_bottom = static_cast<int>(513 / 1000.0f * grid_size);
    int y_left = static_cast<int>(-472 / 1000.0f / 2 * grid_size);
    int y_right = static_cast<int>(472 / 1000.0f / 2 * grid_size);

    cv::Point2f p1(x_top, y_left);
    cv::Point2f p2(x_top, y_right);
    cv::Point2f p3(x_bottom, y_right);
    cv::Point2f p4(x_bottom, y_left);

    cv::Mat rotation_matrix = (cv::Mat_<float>(2, 2) << std::cos(yaw), -std::sin(yaw),
                                                         std::sin(yaw),  std::cos(yaw));

    int delta_size = 100 / 2;
    cv::Point offset(delta_size, delta_size);

    // Use the rotateAndTranslatePoint function for each point
    cv::Point rotated_p1 = rotateAndTranslatePoint(p1, rotation_matrix, offset);
    cv::Point rotated_p2 = rotateAndTranslatePoint(p2, rotation_matrix, offset);
    cv::Point rotated_p3 = rotateAndTranslatePoint(p3, rotation_matrix, offset);
    cv::Point rotated_p4 = rotateAndTranslatePoint(p4, rotation_matrix, offset);

    cv::Point midpoint = (rotated_p3 + rotated_p4) * 0.5f;
    cv::Point centerxy = cv::Point(50, 50);
    frame_data_cur_.car_pose.push_back(midpoint);
    frame_data_cur_.car_pose.push_back(rotated_p1);
    frame_data_cur_.car_pose.push_back(rotated_p2);
    frame_data_cur_.car_pose.push_back(rotated_p3);
    frame_data_cur_.car_pose.push_back(rotated_p4);
    frame_data_cur_.car_pose.push_back(centerxy);

    // std::cout << "Midpoint: (" << midpoint.x << ", " << midpoint.y << ")" << std::endl;
    // std::cout << "Rotated_p1: (" << rotated_p1.x << ", " << rotated_p1.y << ")" << std::endl;
    // std::cout << "Rotated_p2: (" << rotated_p2.x << ", " << rotated_p2.y << ")" << std::endl;
    // std::cout << "Rotated_p3: (" << rotated_p3.x << ", " << rotated_p3.y << ")" << std::endl;
    // std::cout << "Rotated_p4: (" << rotated_p4.x << ", " << rotated_p4.y << ")" << std::endl;


    // cv::Mat image = cv::Mat::zeros(100, 100, CV_8UC3);
    // // 绘制两两相连的线段（形成一个闭合矩形）
    // cv::line(image, rotated_p1, rotated_p2, cv::Scalar(255, 255, 255), 1);
    // cv::line(image, rotated_p2, rotated_p3, cv::Scalar(255, 255, 255), 1);
    // cv::line(image, rotated_p3, rotated_p4, cv::Scalar(255, 255, 255), 1);
    // cv::line(image, rotated_p4, rotated_p1, cv::Scalar(255, 255, 255), 1);
    // cv::Point2f extra_point(50, 50);
    // // cv::circle(image, extra_point, 1, cv::Scalar(0, 255, 0), 1);
    // cv::circle(image, midpoint, 1, cv::Scalar(0, 0, 255), 1);

    // 保存图像为 JPG 文件
    // cv::imwrite("car_pose.jpg", image);

}


void MapLocal::drawCarPose(cv::Mat &image, std::vector<cv::Point> carPostion)
{
    auto m1 = carPostion[0];
    auto p1 = carPostion[1];
    auto p2 = carPostion[2];
    auto p3 = carPostion[3];
    auto p4 = carPostion[4];
    auto c1 = carPostion[5];

    cv::line(image, p1, p2, cv::Scalar(0, 255, 0), 1);
    cv::line(image, p2, p3, cv::Scalar(0, 255, 0), 1);
    cv::line(image, p3, p4, cv::Scalar(0, 255, 0), 1);
    cv::line(image, p4, p1, cv::Scalar(0, 255, 0), 1);

    // cv::circle(image, m1, 1, cv::Scalar(255, 0, 0), 1);
    cv::circle(image, c1, 1, cv::Scalar(0, 255, 0), 1);

}


